#!/home/he/anaconda3/envs/hellobot/bin/python
#  -*-coding:utf8 -*-
import socket  # 导入 socket 模块
import time
import rospy
from std_msgs.msg import String

#w-UP; s-BACK; a-LEFT; d-RIGHT; r-STOP
client_command = 'r'

class Client():
    def __init__(self):

        self.pub = rospy.Publisher('Command', String, queue_size=10)
        rate = rospy.Rate(60)

        # 设置服务器ip地址，端口，buffer容量
        HOST = '192.168.1.105'
        # HOST = socket.gethostname()
        PORT = 8008
        BUFFER = 1
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # 创建 socket 对象
        s.bind((HOST, PORT))  # 绑定端口

        s.listen(5)  # 等待客户端连接
        while not rospy.is_shutdown():
            c, addr = s.accept()  # 建立客户端连接
            print('连接地址：', addr)
            while c:
                # while not rospy.is_shutdown():
                try:
                    c.settimeout(5)
                    result = c.recv(BUFFER)
                    result = result.decode('utf-8')
                    # pub.publish(result)
                    if result == '0':
                        client_command = 'r'
                    else:
                        if result == 'w':
                            client_command = 'w'
                        elif result == 'a':
                            client_command = 'a'
                        elif result == 's':
                            client_command = 's'
                        elif result == 'd':
                            client_command = 'd'
                        elif result == 'r':
                            client_command = 'r'
                        elif result == 'p':
                            client_command = 'p'
                            print("Client quit!")
                            rospy.loginfo("Command from client is: "+client_command+"\n")
                            self.pub.publish(client_command)
                            break
                        rospy.loginfo("Command from client is: "+client_command+"\n")
                        self.pub.publish(client_command)
                    time.sleep(0.01)
                except socket.timeout:
                    # pub.publish("time out")
                    print("time out")

        # while not rospy.is_shutdown():
        #     global client_command
        #     try:
        #         s.settimeout(5)
        #         result = s.recv(BUFFER)
        #         result = result.decode('utf-8')
        #         # pub.publish(result)
        #         print(result)
        #         time.sleep(0.01)
        #         if result == "p":
        #             connect = False
        #     except socket.timeout:
        #         # pub.publish("time out")
        #         print("time out")
        # connect = True
        # while connect:
        #     # while not rospy.is_shutdown():
        #     try:
        #         s.settimeout(5)
        #         result = s.recv(BUFFER)
        #         result = result.decode('utf-8')
        #         # pub.publish(result)
        #         print(result)
        #         time.sleep(0.01)
        #         if result == "stop":
        #             connect = False
        #     except socket.timeout:
        #         # pub.publish("time out")
        #         print("time out")
        # s.close()


if __name__ == '__main__':
    rospy.init_node('CLIENT')
    try:
        Client()
    except rospy.ROSInterruptException:
        pass
    rospy.spin()